#include "OPENMV_Cal.h"
/**在80cm定高飞行条件下1像素对应0.316cm */
#define PIXEL_CM 0.316
int32_t pixel_dx_ = 0;
int32_t pixel_dy_ = 0;
float real_dx_ = 0;
float real_dy_ = 0;
u8 control_state_openmv_localization = 0;
u8 openmv_state_flag = 0;

void User_OPENMV_Localization(float dT_s)
{
    if(control_state_openmv_localization && flag.auto_take_off_land == AUTO_TAKE_OFF_FINISH)
    {
        User_PID_XY_Ctrl(dT_s);
    }
}//作废
void User_OPENMV_DXDY_Cal()
{
    real_dx_ = pixel_dx_ * PIXEL_CM;
    real_dy_ = pixel_dy_ * PIXEL_CM;
		if(control_state_openmv_localization)
		{
				if(real_dx_ < 5 && real_dy_ <5)
				{
					Set_Target_XY(0,0,5);
					//if((x<5)&&(x>-5)&&(y>-5)&&(y<5))
				}
				else
				{
					Set_Target_XY(real_dx_*0.1,real_dy_*0.1,5);
				}
				
		}
    
}

void Set_OPENMV_DXDY(int32_t dx,int32_t dy)
{
    pixel_dx_ = dx;
    pixel_dy_ = dy;
    User_OPENMV_DXDY_Cal();
}

u8 OpenMv_buffer[]={0xA8,0xB8,0xC8};
u8 static len = 1;
void openmv_send_command(u8 value)
{
	if(value==1)                            //Start标志
	{
		control_state_openmv_localization = value;
		control_state_waypoint = 0;
		//USART_SendBytes(USART3, &OpenMv_buffer[1],len);
		//USART_SendBytes(USART3, uint8_t *data, uint16_t len)
		USART_SendData(USART3,0xA8);
	}
	else if (value==2)                                  //Stop标志
	{
		//USART_SendBytes(USART3, &OpenMv_buffer[2],len);
		USART_SendData(USART3,0xB8);
	}
	else if (value==3)                                  //识别标志
	{
		control_state_openmv_localization = 0;
		//control_state_waypoint = 1;
		//USART_SendBytes(USART3, &OpenMv_buffer[3],len);
		USART_SendData(USART3,0xC8);
	}

}